#include "coords.h"
#include "tools.h"
#include <cmath>

using namespace common;

RectCoords UnbiasedTransform::doPolarToRect(
    const PolarCoords& a_polar,
    double a_sigma_phi, double a_sigma_r
    )
{
  /// Смещение рассчитывается по формуле:\n
  /// \f[\mu_a = 
  /// \left[
	/// \begin{array}{c}
  /// E[\tilde{x}|r_m, \phi_m] \\
  /// E[\tilde{y}|r_m, \phi_m] 
	/// \end{array}
	/// \right] = 
  /// \left[
	/// \begin{array}{c}
  /// r_m \cos{\phi_m} (e^{-\sigma{_\phi}^2} - e^{-\sigma{_\phi}^2/2})\\
  /// r_m \sin{\phi_m} (e^{-\sigma{_\phi}^2} - e^{-\sigma{_\phi}^2/2}) 
	/// \end{array}
  /// \right]
	/// \f]
  
  RectCoords r;
  double e1 = std::exp(-SQR(a_sigma_phi)),
         e2 = std::exp(-SQR(a_sigma_phi)/2.0),
         e =  e1 - e2;
  double cos_phi = std::cos(a_polar.phi),
         sin_phi = std::sin(a_polar.phi);

  CoordVector mu_a;
  mu_a << a_polar.r*cos_phi*e, a_polar.r*sin_phi*e; 

  CoordVector coords_dc;
  coords_dc << a_polar.r*cos_phi, a_polar.r*sin_phi;

  r.setArray(coords_dc - mu_a);

  return r;
}
